/**
 * @file gyros.h
 *
 * @date Nov 14, 2009
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2009 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#ifndef GYROS_H_
#define GYROS_H_

#include <ebex_math.h>
#include <random_util.h>

#define _gyro_params(x,_)	\
	_(x,saturation)			/*max value*/\
	_(x,sym_scaling)		/*symmetric scale factor (gain)*/\
	_(x,asym_scaling)		/*asymetric scale factor (gain)*/\
	_(x,misalignment)		\
	_(x,beta)			    /*time constant for bias*/	\
	_(x,bias)				\
	_(x,rate_noise)         /*not a variable*/

EBEX_LOOKUP_TABLE(gyro_param, static);

#define GYRO_TYPE(type,name) type name;
#define GYRO_STRING(type,name) #name,
#define GYRO_OFFSET(group,name) offsetof(gyro_param_t, group.name),
#define GYRO_PARAM_GROUP(name, type) 	\
	union								\
	{									\
		type array[0];					\
		struct							\
		{								\
			_gyro_params(type,GYRO_TYPE)\
		};								\
	} name

typedef struct
{
	GYRO_PARAM_GROUP(sample, random_sample_t);
	GYRO_PARAM_GROUP(value, double);
	GYRO_PARAM_GROUP(min, double);
	GYRO_PARAM_GROUP(max, double);
	GYRO_PARAM_GROUP(mutation_likelihood, double);
} gyro_param_t;

typedef struct
{
	char *name;
	gyro_param_t param;
	bool is_inverted;
	char *filename;
	int column;
	char *delimiters;
} gyro_t;

typedef struct
{
	char *name;
	ebex_3vec_t rpy;
	ebex_quat_t alignment;
	double mutation_likelihood;
	random_sample_t alignment_noise;
	gyro_t gyro[3];
} gyrobox_t;


void ebex_gyro_step(ebex_quat_t *m_result, ebex_quat_t *m_origin, ebex_3vec_t *m_obs, ebex_3vec_t *m_ratenoise, gyrobox_t *m_gyros, double m_sec, bool m_usenoise);
void ebex_gyro_step_backward(ebex_quat_t *m_result, ebex_quat_t *m_origin, ebex_3vec_t *m_obs, ebex_3vec_t *m_ratenoise, gyrobox_t *m_gyros, double m_sec, bool m_usenoise);
#endif /* GYROS_H_ */
